// 电机驱动

#ifndef MOTORDRIVER_HPP
#define MOTORDRIVER_HPP

#include <string>
#include <memory>
#include <vector>
#include <array>
#include <chrono>
#include <mutex>
#include <unordered_map>
#include "utils/SerialCommunicator.hpp"

/*
* 功能码定义(对照表)
* S_VER   = 0x00,    // 读取固件版本和对应的硬件版本
* S_RL    = 0x01,    // 读取相电阻和相电感
* S_PID   = 0x02,    // 读取PID参数
* S_VBUS  = 0x03,    // 读取总线电压
* S_CPHA  = 0x05,    // 读取相电流
* S_ENCL  = 0x07,    // 读取经过线性化校准后的编码器值
* S_TPOS  = 0x08,    // 读取电机目标位置角度
* S_VEL   = 0x09,    // 读取电机实时转速
* S_CPOS  = 0x0A,    // 读取电机实时位置角度
* S_PERR  = 0x0B,    // 读取电机位置误差角度
* S_FLAG  = 0x0D,    // 读取使能/到位/堵转状态标志位
* S_Conf  = 0x0E,    // 读取驱动参数
* S_State = 0x0F,    // 读取系统状态参数
* S_ORG   = 0x10     // 读取正在回零/回零失败状态标志位
*/

/**
* @class MotorDriver
* @brief 电机驱动类，通过串口通信控制电机并读取电机状态
*/
class SerialCommunicator;
class MotorDriver {
private:
    std::unique_ptr<SerialCommunicator> serialComm; // 串口指针
    std::mutex serialMutex;                         // 串口操作互斥锁
    float motorCurPos = 0.0f;                       // 定义实时位置变量

    // 功能码映射表，用于将字符串命令映射到对应的功能码
    const std::unordered_map<std::string, uint8_t> func_code_map = {
        {"S_VER",   0x1F},    // 读取固件版本和对应的硬件版本
        {"S_RL",    0x20},    // 读取相电阻和相电感
        {"S_PID",   0x21},    // 读取PID参数
        {"S_VBUS",  0x24},    // 读取总线电压
		{"S_CPHA",  0x27},	  // 读取相电流
		{"S_ENCL",  0x31},	  // 读取经过线性化校准后的编码器值
		{"S_TPOS",  0x33},    // 读取电机目标位置角度
		{"S_VEL",   0x35},    // 读取电机实时转速
		{"S_CPOS",  0x36},    // 读取电机实时位置角度
		{"S_PERR",  0x37},    // 读取电机位置误差角度
		{"S_FLAG",  0x3A},    // 读取使能/到位/堵转状态标志位
		{"S_ORG",   0x3B},    // 读取正在回零/回零失败状态标志位
		{"S_Conf",  0x42},    // 读取驱动参数
		{"S_State", 0x43}     // 读取系统状态参数
    };

public:
    // 构造函数：初始化串口和LED引脚
    MotorDriver(const std::string &uart_dev);

    // 析构函数：自动释放资源
    ~MotorDriver() = default;

    // 读取驱动板参数
    void readSysParams(uint8_t addr, const std::string &param);

    // 将当前位置清零
    void resetCurPosToZero(uint8_t addr);

    // 解除堵转保护
    void resetClogPro(uint8_t addr);

    // 修改控制模式
    void modifyCtrlMode(uint8_t addr, bool svF, uint8_t ctrl_mode);

    // 为电机使能，并启用多机同步
    void enControl(uint8_t addr, bool state, bool snF);

    // 速度控制
    void velControl(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF);

    // 位置控制
    void posControl(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF);

    // 立即停止
    void stopNow(uint8_t addr, bool snF);

    // 执行多机同步运动命令
    void synchronousMotion(uint8_t addr);

    // 设置单圈回零零点位置并存储设置
    void originSetO(uint8_t addr, bool svF);

    // 修改回零参数
    void originModifyParams(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm,
        uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF);

    // 触发回零
    void originTriggerReturn(uint8_t addr, uint8_t o_mode, bool snF);

    // 强制中断退出回零
    void originInterrupt(uint8_t addr);

    // 实时位置监控
    float realTimeLocation(uint8_t addr);
};

#endif // MOTORDRIVER_HPP